#include "robot_driver.hpp"
#include "math.h"
#include "stdio.h"
#include "string.h"
#include <iostream>

#include "unistd.h"

/**
 * @brief Construct a new diff Driver::diff driver object
 *
 * track_width
 *  V     V
 *  +-----+ <
 *  |  ^  |
 *  |  |  |  wheel_base
 *  |     |
 *  +-----+ <
 *
 * @param wheel_base distance between front and back wheels, in m
 * @param track_width distance between left and right wheels, in m
 * @param wheel_diameter diameter of each wheel, in m
 * @param scale_pos motor position scale factor, binary per degree
 * @param scale_vel motor velocity scale factor, binary per m/s
 */
Driver::Driver(double wheel_base, double track_width, double wheel_diameter, double scale_pos,
			   double scale_vel, std::string remote_ip, std::string local_ip,
			   bool loopback = false)
{
	if (track_width <= 0 || wheel_base <= 0 || wheel_diameter <= 0)
	{
		fprintf(stderr, "invalid geometry parameters for differential driver\n");
		exit(EXIT_FAILURE);
	}

	this->_wheel_base = wheel_base;
	this->_track_width = track_width;
	this->_wheel_diameter = wheel_diameter;
	this->_scale_pos = scale_pos;
	this->_scale_vel = scale_vel;
	this->_loopback = loopback;
	this->sock = socket(AF_INET, SOCK_DGRAM, 0);
	if (this->sock < 0)
	{
		printf("faild to init socket\n");
		exit(EXIT_FAILURE);
	}
	int err;
	err = 1;
	setsockopt(this->sock, SOL_SOCKET, SO_REUSEADDR, &err, sizeof(err));
	struct sockaddr_in local_addr;
	memset(&local_addr, 0x00, sizeof(local_addr));
	local_addr.sin_family = AF_INET;
	local_addr.sin_addr.s_addr = inet_addr(local_ip.c_str());
	local_addr.sin_port = htons(8001);
	err = bind(this->sock, (struct sockaddr *)&local_addr, sizeof(local_addr));
	if (err < 0)
	{
		printf("failed to bind socket to port 8001\n");
		close(this->sock);
		this->sock = -1;
		exit(EXIT_FAILURE);
	}
	this->remote_addr.sin_family = AF_INET;
	this->remote_addr.sin_addr.s_addr = inet_addr(remote_ip.c_str());
	this->remote_addr.sin_port = htons(8005);
	struct timeval timeout = {0, 10000};
	timeout.tv_usec = 10;
	timeout.tv_sec = 0;
	setsockopt(this->sock, SOL_SOCKET, SO_SNDTIMEO, (char *)&timeout, sizeof(timeout));
}

Driver::~Driver()
{
	close(this->sock);
}

int Driver::resolve_to_motor(double linear, double angula, motor_data_t &data)
{
	return 0;
}

int Driver::vxvy_to_posvel(double vx, double vy, int &pos, int &vel)
{
	auto v = sqrt(vx * vx + vy * vy) / this->_wheel_diameter / 3.14159;
	auto d = atan2(vy, vx) / 3.14159 * 180 - 90;
	if (abs(d) > 120)
	{
		d += d > 0 ? -180 : 180;
		v *= -1;
	}
	if (abs(v) < 0.01)
	{
		pos = 0;
		vel = 0;
		return 0;
	}
	pos = d * this->_scale_pos;
	vel = v * this->_scale_vel;
	return 0;
}

/**
 * @brief calculate motor position and speed from robot overall speed and rotate
 *
 * @param linear robot speed of moving forward, m/s, forward if positive
 * @param angula robot rotate speed, rad/s, anticlockwise if positive
 * @param data output motor data
 * @return int
 */
int OmnidirectionalDriver::resolve_to_motor(double linear, double angula, motor_data_t &data)
{
	double x, y;
	// calculate
	y = linear - angula * this->_track_width / 2;
	x = -1 * angula * this->_wheel_base / 2;
	this->vxvy_to_posvel(x, y, data.fl_pos, data.fl_vel);

	y = linear + angula * this->_track_width / 2;
	x = -1 * angula * this->_wheel_base / 2;
	this->vxvy_to_posvel(x, y, data.fr_pos, data.fr_vel);

	y = linear + angula * this->_track_width / 2;
	x = angula * this->_wheel_base / 2;
	this->vxvy_to_posvel(x, y, data.br_pos, data.br_vel);

	y = linear - angula * this->_track_width / 2;
	x = angula * this->_wheel_base / 2;
	this->vxvy_to_posvel(x, y, data.bl_pos, data.bl_vel);
	return 0;
}

int Driver::driver_move(double linear, double angula)
{
	motor_data_t dat = {0, 0, 0, 0, 0, 0, 0, 0};
	printf("linear: %lf, angula: %lf\n", linear, angula);
	this->resolve_to_motor(linear, angula, dat);
	return this->hw_write_driver_data(dat);
}

int Driver::hw_write_driver_data(motor_data_t &data)
{
	printf(">>>>\n\tfl p: %.2lf\tv: %.2lf\tfr p: %.2lf\tv: %.2lf\n\tbl p: %.2lf\tv: %.2lf\tbr "
		   "p: %.2lf\tv: %.2lf\n",
		   data.fl_pos / this->_scale_pos, data.fl_vel / this->_scale_vel,
		   data.fr_pos / this->_scale_pos, data.fr_vel / this->_scale_vel,
		   data.bl_pos / this->_scale_pos, data.bl_vel / this->_scale_vel,
		   data.br_pos / this->_scale_pos, data.br_vel / this->_scale_vel);
	nlohmann::json jout;
	jout["fr_vel"] = data.fr_vel;
	jout["fr_pos"] = data.fr_pos;
	jout["br_vel"] = data.br_vel;
	jout["br_pos"] = data.br_pos;
	jout["bl_vel"] = data.bl_vel;
	jout["bl_pos"] = data.bl_pos;
	jout["fl_vel"] = data.fl_vel;
	jout["fl_pos"] = data.fl_pos;
	jout["control"] = 1;
	auto out = jout.dump();
	std::cout << out << std::endl;
	if (this->_loopback)
	{
		sockaddr_in loopback_addr;
		loopback_addr.sin_family = AF_INET;
		loopback_addr.sin_addr.s_addr = inet_addr("127.0.0.1");
		loopback_addr.sin_port = htons(8001);
		sendto(this->sock, out.c_str(), out.length(), 0, (const sockaddr *)&loopback_addr,
			   sizeof(loopback_addr));
	}
	return sendto(this->sock, out.c_str(), out.length(), 0,
				  (const sockaddr *)&this->remote_addr, sizeof(this->remote_addr));
}
int Driver::hw_write_driver_vel(double linear, double angula)
{
	printf(">>>> linear: %lf,\tangula: %lf\n", linear, angula);
	return 0;
}
int Driver::hw_write_driver_cmd(int cmd)
{
	printf(">>>> cmd: %d\n", cmd);
	return 0;
}

int Driver::write_json(nlohmann::json jout)
{
	auto out = jout.dump().c_str();
	auto len = strlen(out);
	return sendto(this->sock, out, len, 0, (const sockaddr *)&this->remote_addr,
				  sizeof(this->remote_addr));
}

int DifferentialDriver::resolve_to_motor(double linear, double angula, motor_data_t &data)
{
	double w, l, r, v;
	l = this->_wheel_base / 2;
	w = this->_track_width / 2;
	if (linear < 0)
		v = -linear;
	else
		v = linear;
	if (abs(angula) < 0.01)
	{
		// no rotation, move straight forward
		this->vxvy_to_posvel(0, v, data.fl_pos, data.fl_vel);
		this->vxvy_to_posvel(0, v, data.fr_pos, data.fr_vel);
		this->vxvy_to_posvel(0, v, data.bl_pos, data.bl_vel);
		this->vxvy_to_posvel(0, v, data.br_pos, data.br_vel);
	}
	else
	{
		if (angula > 0)
		{
			r = v / angula - w;
			this->vxvy_to_posvel(angula * -l, angula * r, data.fl_pos, data.fl_vel);
			this->vxvy_to_posvel(angula * -l, angula * (r + 2 * w), data.fr_pos, data.fr_vel);
			this->vxvy_to_posvel(angula * l, angula * r, data.bl_pos, data.bl_vel);
			this->vxvy_to_posvel(angula * l, angula * (r + 2 * w), data.br_pos, data.br_vel);
		}
		else
		{
			r = v / -angula - w;
			this->vxvy_to_posvel(-angula * l, -angula * r, data.fr_pos, data.fr_vel);
			this->vxvy_to_posvel(-angula * l, -angula * (r + 2 * w), data.fl_pos, data.fl_vel);
			this->vxvy_to_posvel(angula * l, -angula * r, data.br_pos, data.br_vel);
			this->vxvy_to_posvel(angula * l, -angula * (r + 2 * w), data.bl_pos, data.bl_vel);
		}
	}
	if (linear < 0)
	{
		data.fl_vel *= -1;
		data.fr_vel *= -1;
		data.bl_vel *= -1;
		data.br_vel *= -1;
	}
	return 0;
}

int AckermanDriver::resolve_to_motor(double linear, double angula, motor_data_t &data)
{
	double r, omega, w, l;
	omega = abs(angula);
	if (omega < 0.001)
	{
		vxvy_to_posvel(0, linear, data.fl_pos, data.fl_vel);
		vxvy_to_posvel(0, linear, data.fr_pos, data.fr_vel);
		vxvy_to_posvel(0, linear, data.bl_pos, data.bl_vel);
		vxvy_to_posvel(0, linear, data.br_pos, data.br_vel);
		return 0;
	}
	r = abs(linear) / omega;
	w = this->_track_width / 2;
	l = this->_wheel_base / 2;
	if (angula > 0)
	{
		if (linear > 0)
		{
			vxvy_to_posvel(- omega * l, omega * (r-w), data.fl_pos, data.fl_vel);
			vxvy_to_posvel(- omega * l, omega * (r+w), data.fr_pos, data.fr_vel);
			vxvy_to_posvel(omega * l, omega * (r-w), data.bl_pos, data.bl_vel);
			vxvy_to_posvel(omega * l, omega * (r+w), data.br_pos, data.br_vel);
		}
		else
		{
			vxvy_to_posvel(- omega * l, - omega * (r+w), data.fl_pos, data.fl_vel);
			vxvy_to_posvel(- omega * l, - omega * (r-w), data.fr_pos, data.fr_vel);
			vxvy_to_posvel(omega * l, - omega * (r+w), data.bl_pos, data.bl_vel);
			vxvy_to_posvel(omega * l, - omega * (r-w), data.br_pos, data.br_vel);
			
		}
	}
	else
	{
		if (linear > 0)
		{
			vxvy_to_posvel(omega * l, omega * (r+w), data.fl_pos, data.fl_vel);
			vxvy_to_posvel(omega * l, omega * (r-w), data.fr_pos, data.fr_vel);
			vxvy_to_posvel(- omega * l, omega * (r+w), data.bl_pos, data.bl_vel);
			vxvy_to_posvel(- omega * l, omega * (r-w), data.br_pos, data.br_vel);
		}
		else
		{
			vxvy_to_posvel(omega * l, - omega * (r-w), data.fl_pos, data.fl_vel);
			vxvy_to_posvel(omega * l, - omega * (r+w), data.fr_pos, data.fr_vel);
			vxvy_to_posvel(- omega * l, - omega * (r-w), data.bl_pos, data.bl_vel);
			vxvy_to_posvel(- omega * l, - omega * (r+w), data.br_pos, data.br_vel);
		}
	}
	return 0;
}
